
#ifndef REGISTER_H
#define REGISTER_H

#include <pcl/io/ply_io.h>
#include <pcl/io/pcd_io.h>
#include <pcl/common/io.h>
#include <pcl/point_types.h>
#include "pcl/point_cloud.h"
#include <pcl/features/normal_3d.h>
#include <pcl/registration/icp.h>
#include <pcl/registration/icp_nl.h>
#include <pcl/registration/ndt.h>
#include <pcl/registration/gicp.h>
#include <pcl/filters/radius_outlier_removal.h>
#include <pcl/filters/conditional_removal.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/filters/statistical_outlier_removal.h>
#include <pcl/filters/extract_indices.h>
#include <pcl/kdtree/kdtree_flann.h>
#include <pcl/common/common.h>
#include <pcl/visualization/pcl_visualizer.h>
#include <pcl/filters/random_sample.h>

// #include <fast_gicp/gicp/fast_gicp.hpp>
// #include <fast_gicp/gicp/fast_vgicp.hpp>
// #include <fast_gicp/gicp/fast_gicp_st.hpp>


#include "def.h"

void   ComputeRightRatio(PointCloudXYZ &src, PointCloudXYZ &dst, 
                  Eigen::Matrix4d &mt,
                  double  max_distance,
                  double  &min_ratio, 
                  double  &max_ratio);

void   ComputeFittingScore(PointCloudXYZ &src, PointCloudXYZ &dst, 
                  Eigen::Matrix4d &mt,
                  double& score,
                  double& right_ratio);

void   addNormal(pcl::PointCloud<PT_TYPE> &cloud,
               pcl::PointCloud<pcl::PointXYZINormal> &cloud_with_normals);


template <typename PointT>
double runIterativeICP(pcl::PointCloud<PointT> &source,
                       pcl::PointCloud<PointT> &target,
                       Eigen::Matrix4d &trans_matrix)
{
    pcl::IterativeClosestPoint<PointT, PointT, double> icp;
    pcl::PointCloud<PointT> source_temp = source;
    pcl::PointCloud<PointT> target_temp = target;
    // pcl::transformPointCloud(source,source_temp, trans_matrix);
    // pcl::transformPointCloud(target,target_temp, trans_matrix);
    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(target_temp.makeShared());
    double maxDistance = 1;
    int i = 1;
    // trans_matrix = Eigen::Matrix4d::Identity();
    while (maxDistance > 0.01)
    {
        icp.setInputSource(source_temp.makeShared());
        icp.setInputTarget(target_temp.makeShared());
        icp.setMaxCorrespondenceDistance(maxDistance);
        icp.setMaximumIterations(100);
        icp.setTransformationEpsilon(1e-10);
        icp.setEuclideanFitnessEpsilon(0.001); // 均方误差
        icp.align(source_temp);
        if (icp.hasConverged())
            std::cout << "第" << i << "次迭代  "
                      << " score: " << icp.getFitnessScore();
        else
            continue;
        trans_matrix = trans_matrix * icp.getFinalTransformation(); // * trans_matrix;
        maxDistance = getMAE(kdtree, maxDistance, source_temp.makeShared());
        std::cout << " MAE = " << maxDistance << std::endl;
        i++;
    }
    source = source_temp;
    return maxDistance;
}

template <typename PointT>
double IterativeICP(pcl::PointCloud<PointT> &src,
                    pcl::PointCloud<PointT> &dst,
                    int nMaxIteration,
                    Eigen::Matrix4d &mt)
{
    pcl::PointCloud<PointT> src_temp = src;
    pcl::PointCloud<PointT> dst_temp = dst;
    pcl::KdTreeFLANN<PointT> kdtree;
    kdtree.setInputCloud(dst_temp.makeShared());
    double crspDis = 0.1;
    int maximumIterations = 100;
    double transformationEpsilon = 1e-10;
    double EuclideanFitnessEpsilon = 0.1;

    pcl::IterativeClosestPoint<PointT, PointT, double> icp;
    icp.setMaxCorrespondenceDistance(crspDis);
    icp.setMaximumIterations(maximumIterations);
    icp.setTransformationEpsilon(transformationEpsilon);
    icp.setEuclideanFitnessEpsilon(EuclideanFitnessEpsilon);
    icp.setInputSource(src.makeShared());
    icp.setInputTarget(dst.makeShared());
    double score = -1;
    double max_distance = 2;
    //for(int i=0; i<nMaxIteration; i++)
    int nIteration = 0;
    while (max_distance > 0.1)
    {
        //pcl::PointCloud<PT_TYPE> Final;
        icp.align(src_temp);
        int res = icp.hasConverged();
        //std::cout<<res<<std::endl;
        if (res < 0)
            return -1;
        mt = icp.getFinalTransformation();
        score = icp.getFitnessScore();
        max_distance = getMAE(kdtree, 0.5, src_temp.makeShared());
        std::cout << "score: " << score << " max_distance: " << max_distance << std::endl;
        icp.setInputSource(src_temp.makeShared());
        nIteration++;
        if (nIteration > 10)
            break;
    }
    src = src_temp;
    return score;
}

template <typename PointT>
double PairPointCloudICPNormal(
                    pcl::PointCloud<PointT> &src,
                    pcl::PointCloud<PointT> &dst,
                    double crspDis,
                    double maximumIterations,
                    double transformationEpsilon,
                    double EuclideanFitnessEpsilon,
                    Eigen::Matrix4d &mt)
{
    //adding normals to point cloud
    pcl::PointCloud<pcl::PointXYZINormal> srcWithNormals;
    pcl::PointCloud<pcl::PointXYZINormal> dstWithNormals;
    addNormal(src, srcWithNormals);
    addNormal(dst, dstWithNormals);
    //icp with normals
    pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal,
                                          pcl::PointXYZINormal, double>
        icp;
    icp.setMaxCorrespondenceDistance(crspDis);
    icp.setMaximumIterations(maximumIterations);
    icp.setTransformationEpsilon(transformationEpsilon);
    icp.setEuclideanFitnessEpsilon(EuclideanFitnessEpsilon);
    icp.setInputSource(srcWithNormals.makeShared());
    icp.setInputTarget(dstWithNormals.makeShared());
    pcl::PointCloud<pcl::PointXYZINormal> Final;
    icp.align(Final);
    //src = Final;
    for (int i = 0; i < Final.points.size(); i++)
    {
        src.points[i].x = Final.points[i].x;
        src.points[i].y = Final.points[i].y;
        src.points[i].z = Final.points[i].z;
    }
    int res = icp.hasConverged();
    if (res < 0)
        return -1;
    mt = icp.getFinalTransformation();
    double score = icp.getFitnessScore();
    return score;
}

template <typename PointT>
double PairPointCloudICP(
                    pcl::PointCloud<PointT> &src,
                    pcl::PointCloud<PointT> &dst,
                    double crspDis,
                    double maximumIterations,
                    double transformationEpsilon,
                    double EuclideanFitnessEpsilon,
                    Eigen::Matrix4d &mt,
                    pcl::PointCloud<PointT> &alignedSrc)
{
    pcl::IterativeClosestPoint<PointT, PointT, double> icp;
    icp.setMaxCorrespondenceDistance(crspDis);
    icp.setMaximumIterations(maximumIterations);
    icp.setTransformationEpsilon(transformationEpsilon);
    icp.setEuclideanFitnessEpsilon(EuclideanFitnessEpsilon);
    icp.setInputSource(src.makeShared());
    icp.setInputTarget(dst.makeShared());
    pcl::PointCloud<PointT> Final;
    icp.align(Final);
    //src = Final;
    alignedSrc = Final;
    //pcl::copyPointCloud(Final, alignedSrc);
    int res = icp.hasConverged();
    if (res < 0)
        return -1;
    mt = icp.getFinalTransformation();
    double score = icp.getFitnessScore(crspDis);
    return score;
}

template <typename PointT>
double ICPWithNormal( pcl::PointCloud<PointT> &src,
                      pcl::PointCloud<PointT> &dst,
                      Eigen::Matrix4d &mt,double maxDis=0.5)
{
    double score = 0.0;
    //adding normals to point cloud
    pcl::PointCloud<pcl::PointXYZINormal> srcWithNormals;
    pcl::PointCloud<pcl::PointXYZINormal> dstWithNormals;
    addNormal(src, srcWithNormals);
    addNormal(dst, dstWithNormals);
    //icp with normals
    pcl::IterativeClosestPointWithNormals<pcl::PointXYZINormal,
                                          pcl::PointXYZINormal, double>  icp;
    icp.setMaxCorrespondenceDistance(maxDis);
    // icp.setMaximumIterations(maximumIterations);
    // icp.setTransformationEpsilon(transformationEpsilon);
    // icp.setEuclideanFitnessEpsilon(EuclideanFitnessEpsilon);
    icp.setInputSource(srcWithNormals.makeShared());
    icp.setInputTarget(dstWithNormals.makeShared());
    pcl::PointCloud<pcl::PointXYZINormal> Final;
    icp.align(Final);
    mt = icp.getFinalTransformation();
    score = icp.getFitnessScore();
    return score;
}


// template <typename PointT>
// double fast_gicpMatch( pcl::PointCloud<PointT> &src, pcl::PointCloud<PointT> &dst, 
//                        Eigen::Matrix4d &mt, double maxDistance=0.5)
// {
//     double score = 0;

//     //auto t1 = std::chrono::high_resolution_clock::now();
//     pcl::PointCloud<PointT> aligned; //(new pcl::PointCloud<PointT>());
//     //fast_gicp::FastVGICP<pcl::PointXYZ, pcl::PointXYZ> vgicp;
//     fast_gicp::FastGICPSingleThread<PointT, PointT> vgicp;

//     //vgicp.setResolution(0.25);
//     //int nThread = omp_get_max_threads();
//     //printf("thread: %d \n",nThread);
//     //vgicp.setNumThreads(omp_get_max_threads());
//     vgicp.setMaxCorrespondenceDistance(maxDistance);
//     //vgicp.setMaximumIterations(10);
//     //vgicp.setMaximumIterations(10);
//     //vgicp.setCorrespondenceRandomness(30);
//     vgicp.setInputSource(src.makeShared());
//     vgicp.setInputTarget(dst.makeShared());
//     vgicp.align(aligned);
//     //mt = vgicp.getFinalTransformation();
//     Eigen::Matrix4f mtf = vgicp.getFinalTransformation();
//     for (int j = 0; j < 4; j++)
//     {
//         for (int i = 0; i < 4; i++)
//         {
//             mt(j, i) = mtf(j, i);
//         }
//     }
//     score = vgicp.getFitnessScore(0.5);
//     //qDebug(" icp done \n");
//     // auto t2 = std::chrono::high_resolution_clock::now();
//     // double single = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
//     // std::cout << "fast vgicp :" << single << "[msec] " << std::endl;
//     // //std::cout << mt << std::endl;
//     // std::cout << score << std::endl;
//     return score;
// }

//ndt默认的浮点类型是float，所以初始化的矩阵需要设置为float类型
template <typename PointT>
double NDTMatch(pcl::PointCloud<PointT>& source,
                pcl::PointCloud<PointT>& target,
                Eigen::Matrix4f  init_guess,
                Eigen::Matrix4d& mt
                )
{
    // Initializing Normal Distributions Transform (NDT).
    pcl::NormalDistributionsTransform<PointT, PointT> ndt;
    // Setting scale dependent NDT parameters
    // Setting minimum transformation difference for termination condition.
    ndt.setTransformationEpsilon(0.01);
    // Setting maximum step size for More-Thuente line search.
    ndt.setStepSize(0.1);
    //Setting Resolution of NDT grid structure (VoxelGridCovariance).
    ndt.setResolution(1.0);
    // Setting max number of registration iterations.
    ndt.setMaximumIterations(16);
    // Setting point cloud to be aligned.
    ndt.setInputSource(source.makeShared());
    // Setting point cloud to be aligned to.
    ndt.setInputTarget(target.makeShared());
    // Calculating required rigid transform to align the input cloud to the target cloud.
    pcl::PointCloud<PointT> output_cloud; // (new pcl::PointCloud<PT_TYPE>);
    ndt.align(output_cloud, init_guess);
    //mt = ndt.getFinalTransformation();
    
    Eigen::Matrix4f mtf = ndt.getFinalTransformation();
    for (int j = 0; j < 4; j++)
    {
        for (int i = 0; i < 4; i++)
        {
            mt(j, i) = mtf(j, i);
        }
    }
    //std::cout << "Normal Distributions Transform has converged:" << ndt.hasConverged ()
    //          << " score: " << ndt.getFitnessScore () << std::endl;
    //pcl::transformPointCloud (scan, scan, ndt.getFinalTransformation ());
    return ndt.getFitnessScore();
}

//最简单的icp调用算法,点云的类型和返回的投影矩阵类型都可以动态变化
template <typename PointT>
double rawICP(pcl::PointCloud<PointT> &src,
              pcl::PointCloud<PointT> &dst,
              Eigen::Matrix4d &mt)
{
    //pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    pcl::PointCloud<PointT> aligned;
    pcl::IterativeClosestPoint<PointT, PointT, double> pcl_icp;
    pcl_icp.setInputTarget(dst.makeShared());
    pcl_icp.setInputSource(src.makeShared());
    pcl_icp.setMaximumIterations(10);
    pcl_icp.align(aligned);
    //mSrcPC = *aligned;
    //将匹配结果保存下来    
    mt = pcl_icp.getFinalTransformation();
    double score = pcl_icp.getFitnessScore();

    return score;
}

//gicp 只能返回 Eigen::Matrix4f 的类型，没法用模板参数
template <typename PointT>
double gicpMatch( pcl::PointCloud<PointT> &src, pcl::PointCloud<PointT> &dst, 
                  Eigen::Matrix4d &mt, double maxDistance=0.5)
{
    double score = 0;
    //auto t1 = std::chrono::high_resolution_clock::now();
    //printf("%d %d \n", src.size(), dst.size());

    //pcl::PointCloud<PointT>::Ptr aligned(new pcl::PointCloud<PointT>());
    pcl::GeneralizedIterativeClosestPoint<PointT, PointT> pcl_gicp;
    pcl_gicp.setMaxCorrespondenceDistance(maxDistance);
    pcl_gicp.setInputTarget(dst.makeShared());
    pcl_gicp.setInputSource(src.makeShared());
    pcl::PointCloud<PointT> aligned;
    pcl_gicp.align(aligned);
    
    Eigen::Matrix4f fmt = pcl_gicp.getFinalTransformation();
    for (int jj = 0; jj < 4; jj++)
    {
        for (int ii = 0; ii < 4; ii++)
        {
            mt(jj, ii) = fmt(jj, ii);
        }
    }

    score = pcl_gicp.getFitnessScore();
    bool bIsOk = pcl_gicp.hasConverged();
    if (!bIsOk)
        score = 100000;
    //src = *aligned;
    //auto t2 = std::chrono::high_resolution_clock::now();
    //double single = std::chrono::duration_cast<std::chrono::nanoseconds>(t2 - t1).count() / 1e6;
    //std::cout << "pcl gicp :" << single << "[msec] " << std::endl;
    return score;
}


#endif